import Leap, cv2
from supportFunctions import *
from time import sleep
import numpy as np

# 创建 Leap Motion 控制器对象
controller = Leap.Controller()
# 允许访问原始图像数据
controller.set_policy(Leap.Controller.POLICY_IMAGES)

# 等待一段时间以确保控制器初始化完成
sleep(0.1)

# 获取当前帧和左侧图像
frame = controller.frame()
image = frame.images[0]

# 获取左右图像的畸变矫正参数
left_coordinates, left_coefficients = convert_distortion_maps(frame.images[0])
right_coordinates, right_coefficients = convert_distortion_maps(frame.images[1])

# 获取手的第二个手指的顶端坐标
horizontal_slope = -1 * (frame.hands[0].fingers[1].tip_position.x - 20) / frame.hands[0].fingers[1].tip_position.y
vertical_slope = frame.hands[0].fingers[1].tip_position.z / frame.hands[0].fingers[1].tip_position.y

# 将手指顶端坐标映射到图像坐标
pixel = image.warp(Leap.Vector(horizontal_slope, vertical_slope, 0))

# 输出映射后的图像坐标
print pixel.x, pixel.y

# 检查映射后的坐标是否在图像范围内
if 0 <= pixel.x <= image.width and 0 <= pixel.y <= image.height:
    print '在范围内，执行转换！'
    pixelIndices = [math.floor(pixel.y), math.floor(pixel.x)]

    # 将图像合并为一个带颜色的图像
    mergedImage = np.zeros((image.height, image.width, 3))
    mergedImage[:, :, 0] = rawImage
    mergedImage[:, :, 1] = rawImage
    mergedImage[:, :, 2] = rawImage

    # 在合并图像中标记手指顶端点
    mergedImage[pixelIndices[0], pixelIndices[1], 2] = 255
    mergedImage[pixelIndices[0], pixelIndices[1], 0] = 0
    mergedImage[pixelIndices[0], pixelIndices[1], 1] = 0

    # 使用畸变矫正参数将合并图像映射到目标图像
    destination = cv2.remap(mergedImage, left_coordinates, left_coefficients, interpolation=cv2.INTER_LINEAR)
    destination = cv2.resize(destination, (400, 400), 0, 0, cv2.INTER_LINEAR)

    # 将合并图像和映射后的图像保存
    cv2.imwrite('mergedImage.png', mergedImage)
    cv2.imwrite('mergedMapped.png', destination)
    # cv2.imwrite('cleanLabeledImage.png', cleanLabeledImage)
